#include "CVMatchingAlgos.h"

void getFLANN(const cv::Mat& desc1,
							const cv::Mat& desc2,
							cv::Mat_<int>& links,
							cv::Mat_<float>& dists,
							int precision)
{
	links.create(desc2.rows,1);	
	dists.create(desc2.rows,1);
	cv::flann::Index_<float> index(desc1, cv::flann::KDTreeIndexParams(4));
	index.knnSearch(desc2,links,dists,1,cv::flann::SearchParams(precision));

}

////////////////////////////////////////////////////////////////////////////////

void  cvLabelizeKeyspoints(cv::Mat_<float> & keyspoints,float radius)
{
	if(keyspoints.cols==7 && keyspoints.rows>0)
	{
		//le label courant vaut zero
		int currentLabel=0; 
		
		//On crée une table de correspondance
		cv::Mat_<int> corresp(keyspoints.rows,1);
		
		//Chaque label correspond a lui meme au depart
		for(int i=0;i<keyspoints.rows;i++)corresp(i)=i;

		//Aucun point n'est labelisé
		keyspoints.col(6).setTo(-1.0);
	
		//On parcours la liste des points
		for(int i=0;i<keyspoints.rows;i++)
		{
			//Si le point n'est pas labelisé
			if(keyspoints(i,6)<0)
			{			
				//On lui affecte le label courant
				keyspoints(i,6)= currentLabel;

				//On cherche tout ses voisins
				for(int j=0;j<keyspoints.rows;j++)
				{						
					//Il n'est pas voisin de lui meme
					if(i!=j)
					{	
						//Calcul de la distance au carrée
						float dist2=(keyspoints(i,0)-keyspoints(j,0))*(keyspoints(i,0)-keyspoints(j,0))+
									      (keyspoints(i,1)-keyspoints(j,1))*(keyspoints(i,1)-keyspoints(j,1));

						//Si cette distance et inf au rayon passé en paramettre alor c'est un voisin				
						if(dist2<(radius*radius))
						{
							//Si il n'est pas étiqueté il prend le label courant
							if(keyspoints(j,6)<0)
							{
								keyspoints(j,6)=currentLabel;	
							}

							//Sinon on gere la table d'equivalence
							else if(corresp(currentLabel)!=keyspoints(j,6))
							{
								//pour debuggage:
								//printf("%d %d %f\n",keyspoints.rows,currentLabel,keyspoints(j,6));
								//printf("%d %d %f\n",currentLabel,corresp(currentLabel) , keyspoints(j,6) );
								corresp( MAX( corresp(currentLabel) , keyspoints(j,6) )) = MIN( corresp(currentLabel) , keyspoints(j,6) );
							}
						}
					}
				}
				currentLabel++;
			}
		}		

		// première étape : mise à jour table d'equivalence
		for (int i = 0;i<currentLabel;i++)
		{				
				if (corresp(i) != i) 
				{
				    corresp(i) = corresp(corresp(i));
				}
		}

		// deuxième étape : mise à jour image des labels
		for(int j=keyspoints.rows-1;j>0;j--)
		{
			keyspoints(j,6)= corresp(keyspoints(j,6));
		}

	}
}

////////////////////////////////////////////////////////////////////////////////
void cvKeyspointsOpening(cv::Mat_<float> & keyspoints,int density,float radius)
{
	cv::Mat_<int> todelete(keyspoints.rows,1);
	todelete.setTo(1);

	int count=0;
	for(int i=0;i<keyspoints.rows;i++)
	{
			int n=0;
			for(int j=0;j<keyspoints.rows;j++)
			{
				if(i!=j)
				{
					float dist2=(keyspoints(i,0)-keyspoints(j,0))*(keyspoints(i,0)-keyspoints(j,0))+
							        (keyspoints(i,1)-keyspoints(j,1))*(keyspoints(i,1)-keyspoints(j,1));
					if(dist2<(radius*radius))
					{
						n++;
						if(n>density)
						{
							todelete(i,0)=0;
							count++;
							break;
						}	
					}
				}
			}
	}
	
	cv::Mat_<float> new_keyspoints(0,7);
	count=0;
	for(int i=0;i<keyspoints.rows;i++)
	{
		if(todelete(i,0)==0)
		{
			new_keyspoints.push_back(keyspoints.row(i));
		}
	}
	keyspoints=new_keyspoints.clone();
}

////////////////////////////////////////////////////////////////////////////////
void cvLabelizeKeyspointsCubeST(std::deque<cv::Mat_<float> > & cube,float dtds,float radius)
{
	int currentLabel=0; 

	if((int)cube.size()>0)
	{		
		//on parcours le cube et on met tous les labels à -1
		//et on compte le nombre de points clefs dans le cube
		int nkeyspoints=0;
		for(int i=0;i<(int)cube.size();i++)
		{
				//cube[i].col(6).setTo(-1.0);
				nkeyspoints+=cube[i].rows;
		}
		cv::Mat_<int> corresp(nkeyspoints,1);
		
		//On parcours le cube à la recherche de voisins		
		for(int i=0;i<(int)cube.size();i++)
		{
			for(int j=0;j<cube[i].rows;j++)
			{
				//Si le point n'est pas labelisé 
				if(cube[i](j,6)<0)	
				{
					//On lui affecte le label courant 
					cube[i](j,6)=currentLabel;
					
					//on recherche les voisins
					for(int k=0;k<(int)cube.size();k++)
					{
						for(int l=0;l<cube[k].rows;l++)
						{
							//Il n'est pas voisin de lui meme		
							if(k!=i || l!=j)
							{
								//Mesure de distance entre les deux points clefs
								float dist=(cube[i](j,0)-cube[k](l,0))*(cube[i](j,0)-cube[k](l,0))+//X
													 (cube[i](j,1)-cube[k](l,1))*(cube[i](j,1)-cube[k](l,1))+//Y
													 (i-k)*(i-k)*dtds*dtds;                                  //T
								
								//Si la distance est inferieur au rayon passé en paramettre
								if(dist<(radius*radius))
								{
									//Si le voisin n'est pas labelisé
									if(cube[k](l,6)<0)							
									{
										//On lui donne le label courant
										cube[k](l,6)=currentLabel;
									}
									//Sinon on gere la table d'equivalence
									else if(corresp(currentLabel)!=cube[k](l,6))
									{
										corresp( MAX( corresp(currentLabel) , cube[k](l,6) )) = MIN( corresp(currentLabel) , cube[k](l,6));
									}
								}
							}
						} 
					}
				}
			}
		}

	 //Ré-etiquetage avec la table
		

	// première étape : mise à jour table des étiquettes
	for (int i=0; i<corresp.rows; i++)
	{
		  if (corresp(i) != i) 
			{
		      corresp(i) = corresp(corresp(i));
		  }
	}

	// deuxième étape : mise à jour image des labels
	for(int i=0;i<(int)cube.size();i++)
	{
			for(int j=0;j<cube[i].rows;j++)
			{
		      cube[i](j,6)= corresp(cube[i](j,6));
		  }
	}



	}
}
////////////////////////////////////////////////////////////////////////////////
void  cvLabelizeKeyspointsCubeSTLast(std::deque<cv::Mat_<float> > & cube,float dtds,float radius)
{
	



}

////////////////////////////////////////////////////////////////////////////////
void myvconcat(const cv::Mat_<float> & src, cv::Mat_<float> & dst)
{
	if(src.cols==dst.cols)
	{
		cv::Mat_<float> temp(src.rows+dst.rows,src.cols);
		cv::Mat_<float> roi(temp,cv::Rect(0, 0, dst.cols, dst.rows)); 	
		dst.copyTo(roi);
		cv::Mat_<float> roi2(temp,cv::Rect(0, dst.rows, src.cols, src.rows)); 	
		src.copyTo(roi2);
		dst=temp.clone();
	}
}
